
#include "ros/ros.h"
#include "std_msgs/Float32MultiArray.h"
#include <string>
#include <iostream>
#include <cstdio>
#include <math.h>
#include <sstream>
#include <vector>

#ifdef _WIN32
#include <windows.h>
#else
#include <unistd.h>
#endif

#include "serial/serial.h"
#include "YIS.h"
#include "sensor_msgs/Imu.h"
#include "geometry_msgs/Vector3.h"
#include "geometry_msgs/Quaternion.h"

using std::string;
using std::exception;
using std::cout;
using std::cerr;
using std::endl;
using std::vector;

YIS yis;
ros::Publisher yis_pub,imu_pub;
std_msgs::Float32MultiArray m_yis;

sensor_msgs::Imu m_imu;
geometry_msgs::Vector3 m_angular_velocity;
geometry_msgs::Vector3 m_linear_acceleration;
geometry_msgs::Quaternion m_orientation;

float yis_data[17];
void print_usage()
{
	cerr << "Usage: test_serial {-e|<serial port address>} ";
    cerr << "<baudrate> [test string]" << endl;
}
void enumerate_ports()
{
	vector<serial::PortInfo> devices_found = serial::list_ports();

	vector<serial::PortInfo>::iterator iter = devices_found.begin();

	while( iter != devices_found.end() )
	{
		serial::PortInfo device = *iter++;

		printf( "(%s, %s, %s)\n", device.port.c_str(), device.description.c_str(),
     device.hardware_id.c_str() );
	}
}

int main(int argc, char **argv) {
   ros::init(argc, argv, "yis_serial_new");
   ros::NodeHandle nh;
   yis_pub = nh.advertise<std_msgs::Float32MultiArray>("yis_new", 1000);
   imu_pub=nh.advertise<sensor_msgs::Imu>("imu_data",1000);
   if(argc < 2) {
   	print_usage();
    	return 0;
     }

  // Argument 1 is the serial port or enumerate flag
   string port(argv[1]);

   if( port == "-e" ) {
	  enumerate_ports();
	  return 0;
     }
   else if( argc < 3 ) {
	  print_usage();
	  return 1;
   }

  // Argument 2 is the baudrate
  unsigned long baud = 0;
#if defined(WIN32) && !defined(__MINGW32__)
  sscanf_s(argv[2], "%lu", &baud);
#else
  sscanf(argv[2], "%lu", &baud);
#endif

  // port, baudrate, timeout in milliseconds
  serial::Serial my_serial(port, baud, serial::Timeout::simpleTimeout(7));

  cout << "Is the serial port open?";
  if(my_serial.isOpen())
    cout << " Yes." << endl;
  else
    cout << " No." << endl;

  // Get the Test string
  int count = 0;
  vector<unsigned char> rev;
  unsigned char iFour, iThree, iTwo, iOne;
  string buffer_rx="";
  string result="";
  int result_length = 0;
  while(my_serial.isOpen())
  {
    result = my_serial.read(150);
    cout << result.length() << endl;
    /*if(result.length()<127)
		result_length += result.length();
    if(result.length()==127 || result_length==127)
    {
		cout << "Init Done" << endl;
        break;
    }
	if(result_length>127)
		result_length = 0;
	*/
	if(result.length()==127)
    {
		cout << "Init Done" << endl;
        break;
    }

  }
  while (my_serial.isOpen()&&ros::ok()) {
    result = my_serial.read(150);
    buffer_rx+=result;
    if(buffer_rx.size()<127)
    {
        continue;
    }

    else if(buffer_rx.size()==127)
    {
	//cout << "recv a new data" << endl; 
        for(int i=0;i<buffer_rx.size();i++)
        {
            rev.push_back((char)buffer_rx[i]);
        }
        buffer_rx="";
    }
    else
    {
        for(int i=0;i<127;i++)
        {
            rev.push_back((char)buffer_rx[i]);
        }
        buffer_rx=buffer_rx.substr(127);
	//
    }
    //cout << "[0]: " << (int)rev[0] << "[1]: " << (int)rev[1] << "[2]: " << (int)rev[2] << endl; 
    if(rev[0]==89&&rev[1]==73&&rev[2]==83&&rev[5]==rev[126]&&rev[6]==0x10&&rev[20]==0x11&&rev[34]==0x12&&rev[48]==0x20&&rev[62]==0x30&&rev[76]==0x40&&rev[90]==0x41&&rev[108]==0x42)
    {
	
	yis.timeStamp=(unsigned short)((rev[4] <<8)|rev[3]);
  //0x10加速度 ax
  iFour = rev[7+4];
	iThree = rev[7+3];
	iTwo = rev[7+2];
	iOne = rev[7+1];
	yis.ax = ((int)((iFour << 24) | (iThree << 16) | (iTwo << 8) | iOne))*0.000001;
  //0x10加速度 ay
  iFour = rev[11+4];
	iThree = rev[11+3];
	iTwo = rev[11+2];
	iOne = rev[11+1];
	yis.ay = ((int)((iFour << 24) | (iThree << 16) | (iTwo << 8) | iOne))*0.000001;
  //0x10加速度 az
	iFour = rev[15+4];
	iThree = rev[15+3];
	iTwo = rev[15+2];
	iOne = rev[15+1];
	yis.az = ((int)((iFour << 24) | (iThree << 16) | (iTwo << 8) | iOne))*0.000001;

  //0x11自由加速度 fax
  iFour = rev[21+4];
	iThree = rev[21+3];
	iTwo = rev[21+2];
	iOne = rev[21+1];
	yis.fax = ((int)((iFour << 24) | (iThree << 16) | (iTwo << 8) | iOne))*0.000001;
  //0x11自由加速度 fay
  iFour = rev[25+4];
	iThree = rev[25+3];
	iTwo = rev[25+2];
	iOne = rev[25+1];
	yis.fay = ((int)((iFour << 24) | (iThree << 16) | (iTwo << 8) | iOne))*0.000001;
  //0x11自由加速度 faz
	iFour = rev[29+4];
	iThree = rev[29+3];
	iTwo = rev[29+2];
	iOne = rev[29+1];
	yis.faz = ((int)((iFour << 24) | (iThree << 16) | (iTwo << 8) | iOne))*0.000001;

  //0x12速度增量 dvx
  iFour = rev[35+4];
	iThree = rev[35+3];
	iTwo = rev[35+2];
	iOne = rev[35+1];
	yis.dvx = ((int)((iFour << 24) | (iThree << 16) | (iTwo << 8) | iOne))*0.000001;
  //0x12速度增量 dvy
  iFour = rev[39+4];
	iThree = rev[39+3];
	iTwo = rev[39+2];
	iOne = rev[39+1];
	yis.dvy = ((int)((iFour << 24) | (iThree << 16) | (iTwo << 8) | iOne))*0.000001;
  //0x12速度增量 dvz
	iFour = rev[43+4];
	iThree = rev[43+3];
	iTwo = rev[43+2];
	iOne = rev[43+1];
	yis.dvz = ((int)((iFour << 24) | (iThree << 16) | (iTwo << 8) | iOne))*0.000001;

  //0x20角速度 wx
  iFour = rev[49+4];
	iThree = rev[49+3];
	iTwo = rev[49+2];
	iOne = rev[49+1];
	yis.wx = ((int)((iFour << 24) | (iThree << 16) | (iTwo << 8) | iOne))*0.000001;
  //0x20角速度 wy
  iFour = rev[53+4];
	iThree = rev[53+3];
	iTwo = rev[53+2];
	iOne = rev[53+1];
	yis.wy = ((int)((iFour << 24) | (iThree << 16) | (iTwo << 8) | iOne))*0.000001;
  //0x20角速度 wz
	iFour = rev[57+4];
	iThree = rev[57+3];
	iTwo = rev[57+2];
	iOne = rev[57+1];
	yis.wz = ((int)((iFour << 24) | (iThree << 16) | (iTwo << 8) | iOne))*0.000001;

  //0x30磁场强度 mx
  iFour = rev[63+4];
	iThree = rev[63+3];
	iTwo = rev[63+2];
	iOne = rev[63+1];
	yis.mx = ((int)((iFour << 24) | (iThree << 16) | (iTwo << 8) | iOne))*0.001;
  //0x30磁场强度 my
  iFour = rev[67+4];
	iThree = rev[67+3];
	iTwo = rev[67+2];
	iOne = rev[67+1];
	yis.my = ((int)((iFour << 24) | (iThree << 16) | (iTwo << 8) | iOne))*0.001;
  //0x30磁场强度 mz
	iFour = rev[71+4];
	iThree = rev[71+3];
	iTwo = rev[71+2];
	iOne = rev[71+1];
	yis.mz = ((int)((iFour << 24) | (iThree << 16) | (iTwo << 8) | iOne))*0.001;

  //0x40欧拉角 pitch
  iFour = rev[77+4];
	iThree = rev[77+3];
	iTwo = rev[77+2];
	iOne = rev[77+1];
	yis.pitch = ((int)((iFour << 24) | (iThree << 16) | (iTwo << 8) | iOne))*0.000001;
  //0x40欧拉角 roll
  iFour = rev[81+4];
	iThree = rev[81+3];
	iTwo = rev[81+2];
	iOne = rev[81+1];
	yis.roll = ((int)((iFour << 24) | (iThree << 16) | (iTwo << 8) | iOne))*0.000001;
  //0x40欧拉角 yaw
  iFour = rev[85+4];
	iThree = rev[85+3];
	iTwo = rev[85+2];
	iOne = rev[85+1];
	yis.yaw = ((int)((iFour << 24) | (iThree << 16) | (iTwo << 8) | iOne))*0.000001;

  //0x41四元数 q0
  iFour = rev[91+4];
	iThree = rev[91+3];
	iTwo = rev[91+2];
	iOne = rev[91+1];
	yis.q0 = ((int)((iFour << 24) | (iThree << 16) | (iTwo << 8) | iOne))*0.000001;
  //0x41四元数 q1
  iFour = rev[95+4];
	iThree = rev[95+3];
	iTwo = rev[95+2];
	iOne = rev[95+1];
	yis.q1 = ((int)((iFour << 24) | (iThree << 16) | (iTwo << 8) | iOne))*0.000001;
  //0x41四元数 q2
  iFour = rev[99+4];
	iThree = rev[99+3];
	iTwo = rev[99+2];
	iOne = rev[99+1];
	yis.q2 = ((int)((iFour << 24) | (iThree << 16) | (iTwo << 8) | iOne))*0.000001;
  //0x41四元数 q3
  iFour = rev[103+4];
	iThree = rev[103+3];
	iTwo = rev[103+2];
	iOne = rev[103+1];
	yis.q3 = ((int)((iFour << 24) | (iThree << 16) | (iTwo << 8) | iOne))*0.000001;
  
  //0x42方位增量 dq0
  iFour = rev[109+4]; 
	iThree = rev[109+3];
	iTwo = rev[109+2];
	iOne = rev[109+1];
	yis.dq0 = ((int)((iFour << 24) | (iThree << 16) | (iTwo << 8) | iOne))*0.000001;
  //0x42方位增量 dq1
  iFour = rev[113+4];
	iThree = rev[113+3];
	iTwo = rev[113+2];
	iOne = rev[113+1];
	yis.dq1 = ((int)((iFour << 24) | (iThree << 16) | (iTwo << 8) | iOne))*0.000001;
  //0x42方位增量 dq2
  iFour = rev[117+4];
	iThree = rev[117+3];
	iTwo = rev[117+2];
	iOne = rev[117+1];
	yis.dq2 = ((int)((iFour << 24) | (iThree << 16) | (iTwo << 8) | iOne))*0.000001;
  //0x42方位增量 dq3
  iFour = rev[121+4];
	iThree = rev[121+3];
	iTwo = rev[121+2];
	iOne = rev[121+1];
	yis.dq3 = ((int)((iFour << 24) | (iThree << 16) | (iTwo << 8) | iOne))*0.000001;

	m_yis.data.push_back(yis.timeStamp);
	m_yis.data.push_back(yis.ax);
	m_yis.data.push_back(yis.ay);
	m_yis.data.push_back(yis.az);
  	m_yis.data.push_back(yis.fax);
	m_yis.data.push_back(yis.fay);
	m_yis.data.push_back(yis.faz);
  	m_yis.data.push_back(yis.dvx);
	m_yis.data.push_back(yis.dvy);
	m_yis.data.push_back(yis.dvz);
	m_yis.data.push_back(yis.wx);
	m_yis.data.push_back(yis.wy);
	m_yis.data.push_back(yis.wz);
	m_yis.data.push_back(yis.mx);
	m_yis.data.push_back(yis.my);
	m_yis.data.push_back(yis.mz);
	m_yis.data.push_back(yis.pitch); 
	m_yis.data.push_back(yis.roll);
	m_yis.data.push_back(yis.yaw);
	m_yis.data.push_back(yis.q0);
	m_yis.data.push_back(yis.q1);
	m_yis.data.push_back(yis.q2);
	m_yis.data.push_back(yis.q3);
  	m_yis.data.push_back(yis.dq0);
	m_yis.data.push_back(yis.dq1);
	m_yis.data.push_back(yis.dq2);
	m_yis.data.push_back(yis.dq3);

  m_linear_acceleration.x = yis.fax;
  m_linear_acceleration.y = yis.fay;
  m_linear_acceleration.z = yis.faz;

  m_angular_velocity.x = yis.wx*3.141592/180;
  m_angular_velocity.y = yis.wy*3.141592/180;
  m_angular_velocity.z = yis.wz*3.141592/180;


  //double normal_factor=sqrt((double)(yis.q1*yis.q1)+(yis.q2*yis.q2)+(yis.q3*yis.q3)+(yis.q0*yis.q0));
  double normal_factor=1;
  m_orientation.x=yis.q1/normal_factor;
  m_orientation.y=yis.q2/normal_factor;
  m_orientation.z=yis.q3/normal_factor;
  m_orientation.w=yis.q0/normal_factor;

  m_imu.orientation=m_orientation;
  m_imu.linear_acceleration=m_linear_acceleration;
  m_imu.angular_velocity=m_angular_velocity;
  m_imu.header.frame_id="imu";
  m_imu.header.stamp = ros::Time::now();
  yis_pub.publish(m_yis);
  imu_pub.publish(m_imu);
  m_yis.data.clear();
  }

    vector <unsigned char>().swap(rev);
  }

  return 0; 
}
